#!coding=utf-8
import numpy as np


# 计算其次变换矩阵的逆
# parameter:
#     M: type np.matrix
# return: 
#     inv_T: type np.matrix
def getInverseMatrix(M):
    inv_T=np.matrix([[1,0,0,0],
                        [0,1,0,0],
                        [0,0,1,0],
                        [0,0,0,1.0]])
    inv_T[0:3,0:3]=M[0:3,0:3].T;
    inv_T[0:3,3]=-M[0:3,0:3].T*M[0:3,3]
    return inv_T



# 解析出AGV点位相对于相机坐标系的相对关系
# parameter:
#     point: type geometry_msgs/Pose 
# return: 
#     Transpose_Matrix: type np.matrix  
def quantern2RotationMatrix(point):
    x=point['orientation']['x']
    y=point['orientation']['y']
    z=point['orientation']['z']
    w=point['orientation']['w']
    M=np.matrix([[1-2*y*y-2*z*z,   2*x*y+2*w*z,      2*x*z-2*w*y,    point['position']['x']],
                        [2*x*y-2*w*z,     1-2*x*x-2*z*z,    2*y*z+2*w*x,    point['position']['y']],
                        [2*x*z+2*w*y,     2*y*z-2*w*x,      1-2*x*x-2*y*y,  point['position']['z']],
                        [0,               0,                0,              1]]);
    return M
        